#include "Person.h"
#include "PlayerProxy.h"
#include "PathPlanner.h"
#include <iostream>

const double ToiletPosX = 0.0;
const double ToiletPosY = -3.5;
const double EntranceX = 3.33;
const double EntranceY = 4.5;
const double TableX = -9.0;
const double TableY = 3.0;

Person::Person( const int nId, const Position& refOriPos ) :
m_nId( nId ),
m_stCurPos( refOriPos ),
m_stCurVel(),
m_stOriPos( refOriPos ),
m_lstPath()
{
}

Person::~Person()
{
}

void Person::Action( const int nAction )
{
    double dStartX = 0.0;
    double dStartY = 0.0;
    double dStartYaw = 0.0;
    PlayerProxy::Instance().GetCurPosition( m_nId, dStartX, dStartY, dStartYaw );
    dStartX += m_stOriPos.x;
    dStartY += m_stOriPos.y;
    dStartYaw += m_stOriPos.yaw;

    double dEndX = 0.0;
    double dEndY = 0.0;

    m_lstPath.clear();
    switch ( nAction )
    {
    case PurposeStay:
        dEndX = dStartX;
        dEndX = dStartY;
        break;
    case PurposeGoToToilet:
        dEndX = ToiletPosX;
        dEndY = ToiletPosY;
        break;
    case PurposeLeave:
        dEndX = EntranceX;
        dEndY = EntranceY;
        break;
    case PurposeGoToTable:
        dEndX = TableX;
        dEndY = TableY;
        break;
    }

    PathPlanner::Instance().Plan( m_lstPath, dStartX, dStartY, dEndX, dEndY, 4.0 );

    if ( !m_lstPath.empty() )
    {
        std::cout << "Person" << m_nId << " From [" << dStartX << ", " << dStartY << "] to ["<< dEndX << ", " << dEndY << "]"<< std::endl;
        PlayerProxy::Instance().MoveTo( m_nId, m_lstPath.front().x - m_stOriPos.x, m_lstPath.front().y - m_stOriPos.y );
    }
}

bool Person::IsActionDone()
{
    return m_lstPath.empty();
}

bool Person::StepMove()
{
    if ( IsActionDone() )
    {
        return true;
    }

    if ( PlayerProxy::Instance().ArrivedAt( m_nId, m_lstPath.front().x - m_stOriPos.x, m_lstPath.front().y - m_stOriPos.y ))
    {
        std::cout << "Person" << m_nId << " Arrived at [" << m_lstPath.front().x << ", " << m_lstPath.front().y << "]" << std::endl;
        m_lstPath.pop_front();

        if ( !m_lstPath.empty() )
        {
            PlayerProxy::Instance().MoveTo( m_nId, m_lstPath.front().x - m_stOriPos.x, m_lstPath.front().y - m_stOriPos.y );
            std::cout << "Person" << m_nId << " Going to [" << m_lstPath.front().x << ", " << m_lstPath.front().y << "]" << std::endl;
        }
    }

    return IsActionDone();
}

Position& Person::GetCurPos()
{
    PlayerProxy::Instance().GetCurPosition( m_nId, m_stCurPos.x, m_stCurPos.y, m_stCurPos.yaw );

    return m_stCurPos;
}

Velocity& Person::GetCurVelocity()
{
    PlayerProxy::Instance().GetCurVelocity( m_nId, m_stCurVel.x, m_stCurVel.y, m_stCurVel.yaw );

    return m_stCurVel;
}

Position& Person::GetLatestPos()
{
    PlayerProxy::Instance().GetLatestPosition( m_nId, m_stCurPos.x, m_stCurPos.y, m_stCurPos.yaw );

    return m_stCurPos;
}

Velocity& Person::GetLatestVelocity()
{
    PlayerProxy::Instance().GetLatestVelocity( m_nId, m_stCurVel.x, m_stCurVel.y, m_stCurVel.yaw );

    return m_stCurVel;
}
